
#include "ConstraintSolver.h"
#include "World.h"

namespace p2 {

ConstraintSolver::~ConstraintSolver()
{
}

void ConstraintSolver::preStep(real64 dT, Constraint::Set& constraints)
{
	Constraint::Set::iterator iter = constraints.begin();
    for (; iter != constraints.end(); ++iter) {
        (*iter)->preStep(dT);
    }
}

bool ConstraintSolver::solvePositions(real64 dT, Constraint::Set& constraints, uint32 numIterations)
{
	bool ret = false;
    for (uint32 j = 0; j < numIterations; j++) {
		bool allSolved = true;
		Constraint::Set::iterator iter = constraints.begin();
		for (; iter != constraints.end(); ++iter) {
            bool solved = (*iter)->solvePositions(dT);
            allSolved = allSolved && solved;
		}
		if (allSolved)
		{
			ret = true;
			break;
		}
	}
    Constraint::Set::iterator iter = constraints.begin();
    for (; iter != constraints.end(); ++iter) {
    	(*iter)->updateVelocityChange();
    }
    return ret;
}

bool ConstraintSolver::solveVelocities(real64 dT, Constraint::Set& constraints, uint32 numIterations)
{
	bool ret = false;
    for (uint32 j = 0; j < numIterations; j++) {
		bool allSolved = true;
		Constraint::Set::iterator iter = constraints.begin();
		for (; iter != constraints.end(); ++iter) {
            bool solved = (*iter)->solveVelocities(dT);
            allSolved = allSolved && solved;
		}
		if (allSolved)
		{
			ret = true;
			break;
		}
	}
    Constraint::Set::iterator iter = constraints.begin();
    for (; iter != constraints.end(); ++iter) {
    	(*iter)->updateVelocityChange();
    }
    return ret;
}

}
